#pragma config(Sensor, dgtl1,  button,         sensorTouch)
#pragma config(Sensor, dgtl2,  checker,        sensorSONAR_inch)
#pragma config(Sensor, dgtl4,  leftlimit,      sensorDigitalIn)
#pragma config(Sensor, dgtl5,  rightlimit,     sensorDigitalIn)
#pragma config(Sensor, dgtl6,  leftlimit2,     sensorDigitalIn)
#pragma config(Sensor, dgtl7,  rightlimit2,    sensorDigitalIn)
#pragma config(Sensor, dgtl8,  quadin,         sensorQuadEncoder)
#pragma config(Sensor, dgtl10, sonarin,        sensorSONAR_inch)
#pragma config(Motor,  port2,           leftmotor,     tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor,  port3,           rightmotor,    tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port4,           arm,           tmotorServoContinuousRotation, openLoop)
#pragma config(Motor,  port5,           claw,          tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

 //The front sensors are set to zero when pressed down on the table or level surface.
int left;
int right;
int total;
  //The back sensors are set to zero when pressed down on the table or level surface.
int left2;
int right2;
int total2;
int distance=SensorValue(sonarin);
int counter=0;
//void confirm()
void emergency(){
  motor[arm]=0;
  motor[claw]=0;
  motor[leftmotor]=0;
   motor[rightmotor]=0;


}
void grab(){

  int quad=SensorValue[quadin];

  while(SensorValue[quadin]>=-90){
    quad=SensorValue[quadin];
    motor[arm]=50;

      if(SensorValue[button]==1)
      {emergency();
        break;}
        }


	  motor[arm]=0;
	  motor[claw]=127;
	  motor[leftmotor]=0;
    motor[rightmotor]=0;
    wait1Msec(500);


  while(SensorValue[quadin]<=40){
     quad=SensorValue[quadin];
      motor[arm]=-50;
        if(SensorValue[button]==1){
 emergency();  break;
}}
    motor[arm]=0;
    /*if(SensorValue[checker]>1||){


  }*/

    wait1Msec(5000);
   while(SensorValue[quadin]>=-80){
    quad=SensorValue[quadin];
  motor[arm]=50;
  if(SensorValue[button]==1){
		 emergency(); break;
		  }
    }
     motor[arm]=0;
     motor[claw]=-127;

while(SensorValue[quadin]<=0){
     quad=SensorValue[quadin];
    motor[arm]=-50;
    if(SensorValue[button]==1){
      emergency();    break;
        }
    motor[arm]=0;





}
    motor[leftmotor]=-60;
    motor[rightmotor]=-120;
    wait1Msec(1200);
    motor[leftmotor]=0;
    motor[rightmotor]=0;


motor[arm]=0;
}








void trackBall(){
   distance=SensorValue(sonarin);

   counter++;
   if(counter>60)
   counter=0;

  if(SensorValue(sonarin)==-1)
  {
    if(counter>30){
    motor[leftmotor]=70;
    motor[rightmotor]=120;}
    else{
      motor[leftmotor]=120;
    motor[rightmotor]=70;}


}
  else if(SensorValue(sonarin)<=5){
    motor[leftmotor]=-40;
    motor[rightmotor]=-40;
    wait1Msec(1000);

 }
   //optimal grabbing distance
   else if(SensorValue(sonarin)<=8)
   {
     motor[leftmotor]=0;
    motor[rightmotor]=0;
    grab();

    }
  //too far, move forward
  else if(SensorValue(sonarin)<12){
      motor[leftmotor]=45;
    motor[rightmotor]=45;

}
else{

		if(counter>30){
		    motor[leftmotor]=70;
		    motor[rightmotor]=120;}
		    else{
		      motor[leftmotor]=120;
		    motor[rightmotor]=70;}
		}
  // senses nothing, spin






}

task main(){
int faster=120;
int slower=60;
motor[claw]=-127;
motor[leftmotor]=60;
motor[rightmotor]=120;
SensorValue[quadin]=0;

while(true){
   //motor[arm]=-80;
   distance=SensorValue(sonarin);
        left=SensorValue(leftlimit);
			  right=SensorValue(rightlimit);
			  total=left+right;

			  left2=SensorValue(leftlimit2);
			  right2=SensorValue(rightlimit2);
			  total2=left2+right2;

   //Robot will keep rechecking values until one of the sensors goes off the table.
    //while((total+total2)==0){
    while((total+total2)==0){
			  left=SensorValue(leftlimit);
			  right=SensorValue(rightlimit);
			  total=left+right;

			  left2=SensorValue(leftlimit2);
			  right2=SensorValue(rightlimit2);
			  total2=left2+right2;

			  trackBall();
			  wait1Msec(10);

      }

    motor[leftmotor]=120;
    motor[rightmotor]=60;


     //If the front left sensor goes over the edge, then the robot turns right, going back toward the center of the table.
    if(left==1){

	    motor[leftmotor]=60;
	    motor[rightmotor]=-60;
	    wait1Msec(800);

  }
  //If the front right sensor goes over the edge, then the robot turns left, going back toward the center of the table.
    if(right==1){

	    motor[leftmotor]=-60;
	    motor[rightmotor]=60;
	    wait1Msec(800);

  }
   //If the back left sensor goes over the edge, then the robot turns right, going back toward the center of the table.
  if(left2==1){

	    motor[leftmotor]=120;
	    motor[rightmotor]=60;
	    wait1Msec(800);

  }
   //If the back right sensor goes over the edge, then the robot turns left, going back toward the center of the table.
   if(right2==1){
       motor[leftmotor]=60;
	    motor[rightmotor]=120;
	    wait1Msec(800);



  }
  //After waiting for turn to be completed, the robot will keep going.
      wait1Msec(500);


      motor[leftmotor]=60;
      motor[rightmotor]=90;
}
}
